#include "NotreDameLoader.h"
#include <qfile.h>
#include <iostream>
#include <limits.h>

#include "opencv2/opencv.hpp"

#include <qtextstream.h>
#include <qstringlist.h>
#include <qtextstream.h>
#include <qfile.h>

using namespace std;
using namespace cv;

NotreDameLoader::NotreDameLoader() : Loader()
{
    m_cameras = QList<Camera>();
    m_point_cloud = PointCloud();
}

NotreDameLoader::~NotreDameLoader()
{
}

void NotreDameLoader::load()
{
    // obatining list of images
    QFile file("../../datasets/NotreDame/list.txt");

    if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
    {
        cout << "NotreDame/list.txt couldn't be opened." << endl;
    }

    QTextStream in (&file);
    QString line = in.readLine();

    m_files.clear();

    while(!(line.isNull()))
    {
        m_files.append(line);
        line = in.readLine();
    }

    file.close();

    QFile dataFile("../../datasets/NotreDame/notredame.out");

    if (!dataFile.open(QIODevice::ReadOnly | QIODevice::Text))
    {
        cout << "NotreDame/notredame.out couldn't be opened." << endl;
    }

    QTextStream dataIn (&dataFile);
    QString data;

    data = dataIn.readLine(); // consume the comment

    data = dataIn.readLine(); // numbers

    QStringList list;

    list = data.split(' ');

    int numCamera = list.at(0).toInt();
    int numPoint = list.at(1).toInt();

    // collecting cameras
    QString cameraData[5];

    for(int i=0; i < numCamera; i++)
    {
        cameraData[0] = dataIn.readLine();
        cameraData[1] = dataIn.readLine();
        cameraData[2] = dataIn.readLine();
        cameraData[3] = dataIn.readLine();
        cameraData[4] = dataIn.readLine();

        // K param

        QStringList fList = cameraData[0].split(' ');
        double f = fList.at(0).toDouble();
        double F[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1};
        F[0] = F[4] = f;

        // R param

        QStringList r1List = cameraData[1].split(' ');
        QStringList r2List = cameraData[2].split(' ');
        QStringList r3List = cameraData[3].split(' ');
        double R[9];

        R[0] = r1List.at(0).toDouble();
        R[1] = r1List.at(1).toDouble();
        R[2] = r1List.at(2).toDouble();
        R[3] = r2List.at(0).toDouble();
        R[4] = r2List.at(1).toDouble();
        R[5] = r2List.at(2).toDouble();
        R[6] = r3List.at(0).toDouble();
        R[7] = r3List.at(1).toDouble();
        R[8] = r3List.at(2).toDouble();

        // T param

        QStringList tList = cameraData[4].split(' ');
        double T[3];

        T[0] = tList.at(0).toDouble();
        T[1] = tList.at(1).toDouble();
        T[2] = tList.at(2).toDouble();



        cv::Mat tmat(3, 1, CV_64FC1, T);

        cv::Mat rmat(3, 3, CV_64FC1, R);

        cv::Mat realt = -rmat.t()*tmat;

        Camera camera;
        camera.setCameraPosition(Vector3D(realt.at<double>(0), realt.at<double>(1), realt.at<double>(2)));
        camera.setFilename(m_files[i]);
        m_cameras.append(camera);
    }

    QString pointData[3];
    QList<Point3D> points;

    for(int i=0; i < numPoint; i++)
    {
        pointData[0] = dataIn.readLine();
        pointData[1] = dataIn.readLine();
        pointData[2] = dataIn.readLine();

        //Track * pTrack = new Track();


        // Position
        double p[3];
        QStringList pList = pointData[0].split(' ');

        p[0] = pList.at(0).toDouble();
        p[1] = pList.at(1).toDouble();
        p[2] = pList.at(2).toDouble();

        points.append(Vector3D(p[0],p[1],p[2]));

        /*
        // Color
        int c[3];
        QStringList cList = pointData[1].split(' ');

        c[0] = cList.at(0).toInt();
        c[1] = cList.at(1).toInt();
        c[2] = cList.at(2).toInt();

        // View list
        QStringList vList = pointData[2].split(' ');
        int camera;
        int key;
        float x;
        float y;
        int length = vList.at(0).toInt();
        int index = 1;

        for(int i=0; i < length; i++)
        {
            camera = vList.at(index).toInt();
            index++;
            key = vList.at(index).toInt();
            index++;
            x = vList.at(index).toFloat();
            index++;
            y = vList.at(index).toFloat();
            index++;

            TrackDetail td;

            td.cameraIdx = camera;
            td.keypointIdx = key;
            td.x = x;
            td.y = y;

            pTrack->track_details.append(td);
        }
        */
    }

    m_point_cloud.setPoints(points);
    dataFile.close();
}
